#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/i2c_master.h"
#include "esp_log.h"
#include "qmi8658.h"

#define TAG "qmi8658"
#define QMI8658_I2C_ADDRESS (0x6A)

i2c_master_dev_handle_t qmi8658_handle;

esp_err_t qmi8658_register_read(uint8_t reg_addr, uint8_t *data, size_t len)
{
    return i2c_master_transmit_receive(qmi8658_handle, &reg_addr, 1, data, len, 1000);
}

esp_err_t qmi8658_register_write_byte(uint8_t reg_addr, uint8_t data)
{
    uint8_t write_buf[2] = {reg_addr, data};

    return i2c_master_transmit(qmi8658_handle, write_buf, 2, 1000);
}

void qmi8658_init(void)
{
    int ret = ESP_OK;
    i2c_master_bus_handle_t i2c_handle;
    i2c_master_get_bus_handle(CONFIG_I2C_PORT, &i2c_handle);
    // config i2c port
    ret = i2c_master_probe(i2c_handle, QMI8658_I2C_ADDRESS, 50);
    if (ret != ESP_OK)
    {
        ESP_LOGE(TAG, "no find qmi8658, address:0x%02X", QMI8658_I2C_ADDRESS);
        return;
    }
    i2c_device_config_t i2c_dev_conf = {
        .scl_speed_hz = CONFIG_I2C_SPEED_KHZ * 1000,
        .device_address = QMI8658_I2C_ADDRESS,
    };

    if (i2c_master_bus_add_device(i2c_handle, &i2c_dev_conf, &qmi8658_handle) != ESP_OK)
    {
        ESP_LOGE(TAG, "mount qmi8658 fail");
        return;
    }
    ESP_LOGI(TAG, "mount qmi8658 success");
    uint8_t id = 0;
    qmi8658_register_read(QMI8658_WHO_AM_I, &id, 1);
    while (id != 0x05)
    {
        vTaskDelay(1000 / portTICK_PERIOD_MS);
        qmi8658_register_read(QMI8658_WHO_AM_I, &id, 1);
    }
    ESP_LOGI(TAG, "QMI8658 OK!");
    qmi8658_register_write_byte(QMI8658_RESET, 0xb0); // 复位
    vTaskDelay(10 / portTICK_PERIOD_MS);
    qmi8658_register_write_byte(QMI8658_CTRL1, 0x40); // CTRL1 设置地址自动增加
    qmi8658_register_write_byte(QMI8658_CTRL7, 0x03); // CTRL7 允许加速度和陀螺仪
    qmi8658_register_write_byte(QMI8658_CTRL2, 0x95); // CTRL2 设置ACC 4g 250Hz
    qmi8658_register_write_byte(QMI8658_CTRL3, 0xd5); // CTRL3 设置GRY 512dps 250Hz
}
